#include "parseCMDDebug.hpp"

#include "parseCmdMCode.hpp"
#include "../Global/globalVars.hpp"
#include "../Misc/misc.hpp"
#include "../TempControl/tempUpdate.hpp"
#include "../TempControl/powerDrive.hpp"
#include "../main/onlineLoop.hpp"
#include "../Interface/report.hpp"
#include "../LedControl/ledControl.h"

bool debug_drive(const char* cmdIn);
void debug_setPwmFreq(const char* cmdIn);
void debug_pwmWwing(const char* cmdIn);
void debugReadTemp(const char* cmdIn);
void debugFanStartTest(const char* cmdIn);
void dbg_pin_monitor();
void dbg_pin(const char* cmdIn);
void dbg_setLedStyle(const char* cmdIn);
void dbg_setLedMode(const char* cmdIn);
bool debug_sub_pwm_drive(const char* cmdIn);
bool debug_pwm_decrease(const char* cmdIn);
bool debug_pwm_increase(const char* cmdIn);

#define TEST_COOL_FAN_OUTPUT 80

/*
Test pcnt
dbg_pwm 1,50,0 // set pwm output
? // read pwm input
dbg_pfreq 0,300
remember we have TRIGGER_PER_ROUND=2 thus counted frequency is only half
*/
bool parseCMDDebug(const char* cmdIn)
{
	if (!StartWith(cmdIn, "dbg"))
	{
		return false;
	}
	cmdIn += sizeof("dbg");
	// dbg_pwm 1,50,0;
	if (StartWith(cmdIn, "pwm"))
	{
		cmdIn += sizeof("pwm");
		debug_drive(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "sub_pwm"))
	{
		cmdIn += sizeof("sub_pwm");
		debug_sub_pwm_drive(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "dec_pwm"))
	{
		cmdIn += sizeof("dec_pwm");
		debug_pwm_decrease(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "inc_pwm"))
	{
		cmdIn += sizeof("inc_pwm");
		debug_pwm_increase(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "simu"))
	{
		cmdIn += sizeof("simu");
		if (cmdIn[0] == '?') {
			flushReply("Current simu mode: " + String(GlobalVars::flagSimuRate));
		}
		else {
			int flag = atoi(cmdIn);
			GlobalVars::flagSimuRate = flag;
			flushReply("New simu mode: " + String(GlobalVars::flagSimuRate));
			if (GlobalVars::flagSimuRate > 0)
			{
				GlobalVars::plateTemp.setSimuRandom(10, 37);
				GlobalVars::sampleLeftTemp.setSimuRandom(8, 37);
				GlobalVars::sampleRightTemp.setSimuRandom(9, 37);
				GlobalVars::pumpTemp.setSimuRandom(12, 35);
				GlobalVars::containerTemp.setSimuRandom(14, 33);
				GlobalVars::surroundingTemp.setSimuRandom(25, 27.5);
				GlobalVars::coreTemp.setSimuRandom(70, 40);
			}
		}
		return true;
	}
	else if (StartWith(cmdIn, "verbose"))
	{
		cmdIn += sizeof("verbose");
		int flag = atoi(cmdIn);
		GlobalVars::flagVerboseMode = flag;
		flushReply("Verbose mode: " + String(GlobalVars::flagVerboseMode));
		return true;
	}
	// bookmark: report: full detail
	else if (StartWith(cmdIn, "auto_report_full"))
	{
		cmdIn += sizeof("auto_report_full");
		int flag = atoi(cmdIn);
		GlobalVars::flagAutoReportInterval = flag;
		flushReply("Auto report @ interval: " + String(GlobalVars::flagAutoReportInterval));
		return true;
	}
	// bookmark: report: curve
	else if (StartWith(cmdIn, "auto_print_curve"))
	{
		cmdIn += sizeof("auto_print_curve");
		int flag = atoi(cmdIn);
		GlobalVars::flagAutoReportCurve = flag;
		flushReply("Auto report @ curve: " + String(GlobalVars::flagAutoReportCurve));
		return true;
	}
	// bookmark: report: heat bed
	else if (StartWith(cmdIn, "auto_print"))
	{
		cmdIn += sizeof("auto_print");
		if (StartWith(cmdIn, "bed")) {
			GlobalVars::flagAutoPrintSingle = 'b';
			flushReply("Auto print bed");
		}
		else if (StartWith(cmdIn, "none")) {
			GlobalVars::flagAutoPrintSingle = 0;
			flushReply("Auto print stop");
		}
		return true;
	}
	// pfreq 0,310;
	else if (StartWith(cmdIn, "pfreq"))
	{
		cmdIn += sizeof("pfreq");
		debug_setPwmFreq(cmdIn);
		return true;
	}
	// pswing 0,20,100;
	else if (StartWith(cmdIn, "pswing"))
	{
		cmdIn += sizeof("pswing");
		debug_pwmWwing(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "read_temps"))
	{
		Serial.println("Plate");
		TempReader::readPlateTmp(true);
		Serial.println("Container");
		TempReader::readContainerTmp(true);
		Serial.println("Pump");
		TempReader::readPumpTmp(true);
		Serial.println("Surrounding");
		TempReader::readSurroundingTmp(true);
		Serial.println("Internal");
		TempReader::readInternalTmp(true);
		return true;
	}
	else if (StartWith(cmdIn, "read_temp"))
	{
		cmdIn += sizeof("read_temp");
		debugReadTemp(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "fan"))
	{
		cmdIn += sizeof("fan");
		debugFanStartTest(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "pin_monitor"))
	{
		dbg_pin_monitor();
		return true;
	}
	else if (StartWith(cmdIn, "pin"))
	{
		cmdIn += sizeof("pin");
		dbg_pin(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "meta")) {
		Serial.print(Report::reportMetadata());
		Serial.flush();
		return true;
	}
	else if (StartWith(cmdIn, "led mode")) {
		cmdIn += sizeof("led mode");
		dbg_setLedMode(cmdIn);
		return true;
	}
	else if (StartWith(cmdIn, "led")) {
		cmdIn += sizeof("led");
		dbg_setLedStyle(cmdIn);
		return true;
	}
	return false;
}

// pwmX,Y,Z;
// X=channel, Y=duty, Z=seconds
// dbg pwm 1,50,1;\n
bool debug_drive(const char* cmdIn)
{
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	int driveCn, driveDuty, driveT;
	if (intNum == 3)
	{
		driveCn = intVals[0];
		driveDuty = intVals[1];
		driveT = intVals[2];
		delete[] intVals;
	}
	else
	{
		delete[] intVals;
		return false;
	}

	Serial.print("To drive channel{" + String(driveCn) + "}");
	Serial.print(" at duty {" + String(driveDuty) + "}");
	Serial.println(" for {" + String(driveT) + "}s(" + String(driveT > 0) + ");");
	if (driveCn == 1)
	{
		PowerDrive::setChannelPercent((Channel)Fan, (SubChannel)All, TEST_COOL_FAN_OUTPUT);
	}
	GlobalVars::ctrlPlan.tempFeedbackMode = (PowerDrive::setChannelPercentDur(driveCn, All,driveDuty, driveT) ? 1 : 0);
	
	return true;
}

// dec_pwm X,Y,Z;
// X=channel, Y=duty, Z=seconds
// dbg dec_pwm 0,50,10;\n
bool debug_pwm_decrease(const char* cmdIn)
{
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	int driveCn, driveDuty, driveT;
	if (intNum == 3)
	{
		driveCn = intVals[0];
		driveDuty = intVals[1];
		driveT = intVals[2];
		delete[] intVals;
	}
	else
	{
		delete[] intVals;
		return false;
	}

	Serial.print("To drive channel{" + String(driveCn) + "}");
	Serial.print(" at duty {" + String(driveDuty) + "}");
	Serial.print(" to { 0 }");
	Serial.println(" for {" + String(driveT) + "}s(" + String(driveT > 0) + ");");
	if (driveCn == 1)
	{
		PowerDrive::setChannelPercent((Channel)Fan, (SubChannel)All, TEST_COOL_FAN_OUTPUT);
	}
	GlobalVars::ctrlPlan.tempFeedbackMode = (PowerDrive::setChannelPercentDecrease(driveCn, All, driveDuty, driveT) ? 1 : 0);

	return true;
}

// inc_pwm X,Y,Z;
// X=channel, Y=duty, Z=seconds
// dbg inc_pwm 0,50,10;\n
bool debug_pwm_increase(const char* cmdIn)
{
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	int driveCn, driveDuty, driveT;
	if (intNum == 3)
	{
		driveCn = intVals[0];
		driveDuty = intVals[1];
		driveT = intVals[2];
		delete[] intVals;
	}
	else
	{
		delete[] intVals;
		return false;
	}

	Serial.print("To drive channel{" + String(driveCn) + "}");
	Serial.print(" at duty { 0 }");
	Serial.print(" to {" + String(driveDuty) + " }");
	Serial.println(" for {" + String(driveT) + "}s(" + String(driveT > 0) + ");");
	if (driveCn == 1)
	{
		PowerDrive::setChannelPercent((Channel)Fan, (SubChannel)All, 50);
	}
	GlobalVars::ctrlPlan.tempFeedbackMode = (PowerDrive::setChannelPercentIncrease(driveCn, All, driveDuty, driveT) ? 1 : 0);

	return true;
}

// sub_pwm A,B,C,D;
// A=channel, B=sub, C=duty, D=seconds
//dbg sub_pwm 1,0,50,10
bool debug_sub_pwm_drive(const char* cmdIn)
{
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	int driveCn, driveSub, driveDuty, driveT;
	if (intNum == 4)
	{
		driveCn = intVals[0];
		driveSub = intVals[1];
		driveDuty = intVals[2];
		driveT = intVals[3];
		delete[] intVals;
	}
	else
	{
		delete[] intVals;
		return false;
	}
	Serial.print("To drive channel{" + String(driveCn) + "}");	
	Serial.print("in the part of{" + String(driveSub) + "}");
	Serial.print(" at duty {" + String(driveDuty) + "}");
	Serial.println(" for {" + String(driveT) + "}s(" + String(driveT > 0) + ");");

	GlobalVars::ctrlPlan.tempFeedbackMode = (PowerDrive::setChannelPercentDur(driveCn, driveSub, driveDuty, driveT) ? 1 : 0);
	return true;
}
// dbg_pfreq 0,300;
// dbg_pfreq ?;
void debug_setPwmFreq(const char* cmdIn)
{
	if (cmdIn[0] == '?')
	{
		Serial.println("PWM Ch0={" + String(PowerDrive::getChannelFreq(0, All)) + "}; Ch1={" + String(PowerDrive::getChannelFreq(1, All)) + "};");
		return;
	}
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	//int driveCn, dutyLow, dutyHigh;
	if (intNum == 2)
	{
		Serial.println("Set pwn channel {" + String(intVals[0]) + "} freq {" + String(intVals[1]) + "};");
		PowerDrive::setChannelFreq(intVals[0],All, intVals[1]);
	}
	delete[] intVals;
}

// pswing0,20,100;
void debug_pwmWwing(const char* cmdIn)
{
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	int driveCn, percLow, percHigh;
	if (intNum == 3)
	{
		driveCn = intVals[0];
		percLow = intVals[1];
		percHigh = intVals[2];
		delete[] intVals;
	}
	else
	{
		delete[] intVals;
		return;
	}

	Serial.print("To swing channel{" + String(driveCn) + "}");
	Serial.print(" between percent {" + String(percLow) + "}");
	Serial.println(" and {" + String(percHigh) + "};");

	for (int perc = percLow; perc <= percHigh; perc++)
	{
		PowerDrive::setChannelPercent((Channel)driveCn, All, perc);
		delay(250);
		Serial.println("Percent " + String(perc) + "};");
		if (Serial.available())
		{
			break;
		}
	}
	Serial.println("Done;");
	PowerDrive::setChannelPercent((Channel)driveCn, All, 0);
}

// read_temp_plate;
void debugReadTemp(const char* cmdIn)
{
	if (StartWith(cmdIn, "plate"))
	{
		Serial.println("Read plate temp");
		for (int idx = 0; Serial.available() == 0; idx++)
		{
			float _plateTemp = TempReader::readPlateTmp();
			Serial.println("#" + int2Str(idx, 3) + ", " + float2Str(_plateTemp, 3) + " @" + String(millis()));
			if (idx >= 1000)
			{
				idx = 0;
			}
		}
	}
}

// dbg_fan 2000,50,5
// launch_duration, holdPercent, holdDuration
void debugFanStartTest(const char* cmdIn)
{
	Serial.println("Fan start test");
	long* intVals;
	int intNum;
	myParseInts(cmdIn, &intVals, &intNum);
	if (intNum == 3)
	{
		Serial.println("Paras valid: ");
		const int channel = 1;
		long launchDuration = intVals[0];
		long holdPercent = intVals[1];
		long holdDuration = intVals[2];
		PowerDrive::enable();
		PowerDrive::setChannelPercent((Channel)channel,All, 100);
		{
			unsigned long startTime = millis();
			while (millis() - startTime < launchDuration)
			{
				OLLoop::fanSpeedUpdate();
				OLLoop::reportState(true);
				delay(50);
			}
		}
		Serial.println("Started. Hold on...");
		PowerDrive::setChannelPercent((Channel)channel, All, holdPercent);
		{
			unsigned long startTime = millis();
			while (millis() - startTime < holdDuration * 1000)
			{
				OLLoop::fanSpeedUpdate();
				OLLoop::reportState(true);
				delay(50);
			}
		}
		PowerDrive::setChannelPercent((Channel)channel, All, 0);
		PowerDrive::disable();
		Serial.println("All done");
	}
}

void dbg_pin_monitor() {
	if (GlobalVars::fan0TrigsIdx < 0) {
		GlobalVars::fan0TrigsIdx = 0;
	}
	if (GlobalVars::fan1TrigsIdx < 0) {
		GlobalVars::fan1TrigsIdx = 0;
	}
	if (GlobalVars::fan2TrigsIdx < 0) {
		GlobalVars::fan2TrigsIdx = 0;
	}
	if (GlobalVars::fan0TrigsIdx >= GlobalVars::trigRecNum) {
		Serial.print("Fan0T=[");
		for (int idx = 0; idx < GlobalVars::fan0TrigsIdx; idx++) {
			Serial.print(GlobalVars::fan0Trigs[idx]);
			Serial.print(",");
		}
		Serial.println("];");
		GlobalVars::fan0TrigsIdx = -1;
	}
	if (GlobalVars::fan1TrigsIdx >= GlobalVars::trigRecNum) {
		Serial.print("Fan1T=[");
		for (int idx = 0; idx < GlobalVars::fan1TrigsIdx; idx++) {
			Serial.print(GlobalVars::fan1Trigs[idx]);
			Serial.print(",");
		}
		Serial.println("];");
		GlobalVars::fan1TrigsIdx = -1;
	}
	if (GlobalVars::fan2TrigsIdx >= GlobalVars::trigRecNum) {
		Serial.print("Fan2T=[");
		for (int idx = 0; idx < GlobalVars::fan2TrigsIdx; idx++) {
			Serial.print(GlobalVars::fan2Trigs[idx]);
			Serial.print(",");
		}
		Serial.println("];");
		GlobalVars::fan2TrigsIdx = -1;
	}
	//int lastState = digitalRead(FAN_SPEED_PIN_1);
	//int64_t startTime = esp_timer_get_time();
	//int64_t lastFlipTime = esp_timer_get_time();
	//while (esp_timer_get_time() - startTime < 5000000) {
	//	int newState = digitalRead(FAN_SPEED_PIN_1);
	//	if (newState != lastState) {
	//		lastState = newState;
	//		Serial.println(esp_timer_get_time() - lastFlipTime);
	//		lastFlipTime = esp_timer_get_time();
	//	}
	//}
	//Serial.println("End");
	//GlobalVars::de
}

void dbg_pin(const char* cmdIn)
{
	setPin(cmdIn + 4);
}

// dbg led {015}{00000255}{000000}{128};
// dbg led {011}{00000255}{000000}{128};
// dbg led {015}{00000255}{000000}{128};
void dbg_setLedStyle(const char* cmdIn) {
	LedControl::setStyle(cmdIn);
}

void dbg_setLedMode(const char* cmdIn) {
	Serial.print("Set led mode ");

	LedControl::unlockModes();
	if (StartWith(cmdIn, "unlock")) {
		Serial.println("unlock");
		return;
	}
	else if (StartWith(cmdIn, "lock")) {
		Serial.println("lock");
	}
	else if (StartWith(cmdIn, "idle")) {
		LedControl::setModeIdle();
		Serial.println("idle");
	}
	else if (StartWith(cmdIn, "identify")) {
		LedControl::setModeIdentify();
		Serial.println("identify");
	}
	else if (StartWith(cmdIn, "heatUp")) {
		LedControl::setModeHeatUp();
		Serial.println("heatUp");
	}
	else if (StartWith(cmdIn, "heatHold")) {
		LedControl::setModeHeatHold();
		Serial.println("heatHold");
	}
	else if (StartWith(cmdIn, "coolDown")) {
		LedControl::setModeCoolDown();
		Serial.println("coolDown");
	}
	else if (StartWith(cmdIn, "coolHold")) {
		LedControl::setModeCoolHold();
		Serial.println("coolHold");
	}
	else if (StartWith(cmdIn, "error")) {
		cmdIn += sizeof("error");
		if (StartWith(cmdIn, "unlock")) {
			LedControl::setModeError(false);
		}
		else {
			LedControl::setModeError();
		}
		Serial.println("error");
	}
	else {
		Serial.println("UNDEFINED");
	}
	LedControl::lockModes();
}